#include "modules/CtrlModule/PassivePlanner/BumperPlanner.h"
#include "lib/WrapFuncs.h"
#include "data_struct/geometry/Point.h"
#include "data_struct/robot/Pose.h"
#include "lib/Time.h"

namespace behavior_controller
{
BumperPlanner bumper_planner;

void BumperPlanner::run(double &linear_vel, double &angular_vel)
{
    if (finished_turn)
    {
        if (WARNING_FLAG > 0)
        {
            dst_angle = Funcs::NormalizeAngle(DataSet::robot_pose.angle() - global_goal_angle);
            if (fabs(dst_angle) >= turn_error1)
            {
                std::cout << "go to Turn!" << std::endl;
                finished_turn = false;
            }
        }
    }

    if (USE_ROBOT_FRAME && WARNING_FLAG > 0)
    {
        BackAndTurn(linear_vel, angular_vel, global_goal_angle);
        return;
    }

    if (!finished_turn)
    {
        dst_angle = Funcs::NormalizeAngle(DataSet::robot_pose.angle() - global_goal_angle);
        if (fabs(dst_angle) >= turn_error2)
        {
            Turn(linear_vel, angular_vel, dst_angle);
        } else
        {
            finished =
            finished_turn =
            time_now =
            time_now2 = 0;
            LAST_WARNING_FLAG = WARNING_FLAG;
            WARNING_FLAG = 0;
            std::cout << "BumperPlanner finished !" << std::endl;
            return;
        }
    }
    if (finished_turn)
    {
        BackAndTurn(linear_vel, angular_vel);
        return;
    }
}

void BumperPlanner::Turn(double &linear_vel, double &angular_vel, double dst_angle)
{
    linear_vel = angular_vel = 0;
    if (time_now2 < 1e-4) //如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
        time_now2 = Time::Now();
    if (Time::Now() - time_now2 < 0.6 && WARNING_FLAG == 0) //1s内后退
    {
        linear_vel = -0.2;
        return;
    }
    if (dst_angle > 0.17 || dst_angle < -0.17) //当角度差相差0.1弧度时，1deg=0.017453292rad
    {
        angular_vel = -dst_angle;
    } else if (dst_angle > turn_error2 && dst_angle < 0.17) //当角度差小于0.1弧度时,大于0.03弧度时
    {
        angular_vel = -0.15;
    } else if (dst_angle > -0.17 && dst_angle < -turn_error2) //当角度差小于0.1弧度时,大于0.052359876弧度(~3度)时
    {
        angular_vel = 0.15;
    }
}

void BumperPlanner::BackAndTurn(double &linear_vel, double &angular_vel, double angle)
{
    double turn_time = 0;
    double vel_angle = 0.4;
    if (DataSet::clean_mode == FOLLOW_MODE) //沿边模式要比导航模式的转向时间要大，通常导航模式会规划路径避开障碍，低矮障碍物首次需要通过Bumper标记障碍物地图
        turn_time = 1.8;
    else if (DataSet::clean_mode == NAVI_MODE)
        turn_time = 1.5;

    if (USE_ROBOT_FRAME && WARNING_FLAG > 0)
    {
        turn_time = fabs(angle / vel_angle * 0.5);
        std::cout << "USE_ROBOT_FRAME: " << " turn_time=" << turn_time << std::endl;
    }

    if (time_now < 1e-4) //如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
        time_now = Time::Now();
    if (Time::Now() - time_now < 0.5 * turn_time && WARNING_FLAG == 0) //1s内后退
    {
        linear_vel = -0.2;
        angular_vel = 0;
    } else if (Time::Now() - time_now < turn_time) //左转
    {
        linear_vel = 0;
        angular_vel = vel_angle;
    } else if (Time::Now() - time_now >= turn_time) //第3s结束
    {
        linear_vel =
        angular_vel =
        finished =
        finished_turn =
        time_now =
        time_now2 = 0;
        LAST_WARNING_FLAG = WARNING_FLAG;
        WARNING_FLAG = 0;
        std::cout << "finished !" << std::endl;
    }
}

bool BumperPlanner::Check(double distance_front, double distance_left)
{
    PointCloud<double> local_point_cloud;
    static double dis_pow = 0.08 * 0.08;
    static double TAN30 = std::tan(1 / 6 * 3.1415926);
    for (auto pt : DataSet::scan_point_cloud)
    {
        PointF local_point(PoseF(pt, 0).GetPoint());
        if (local_point.GetPoint()(0) > 0 && local_point.GetPoint()(0) <= distance_front &&
            fabs(local_point.GetPoint()(1)) < distance_left)
        {
            if (local_point_cloud.size() > 0)
            {
                if (local_point_cloud[local_point_cloud.size() - 1].DistanceSquare(local_point) > dis_pow)
                    break;
                local_point_cloud.emplace_back(local_point);
            }
            else
                local_point_cloud.emplace_back(local_point);
        }
    }
    if (local_point_cloud.size() > 20)
    {
        double x_mean, y_mean, line_k, line_b, R;
        Funcs::LineFit(local_point_cloud, line_k, line_b, R, x_mean, y_mean);
        if (!std::isnan(R) && fabs(R) > 0.5)
        {
            PoseF robot_pose_(0.0f, 0.0f, .0f), out_point(0.0f, 0.0f, .0f);
            Funcs::GetInlier(line_k, -1, line_b, robot_pose_, out_point);
            double local_angle = std::atan2(out_point.y() - robot_pose_.y(), out_point.x() - robot_pose_.x());
            global_goal_angle = Funcs::NormalizeAngle(DataSet::robot_pose.angle() + local_angle + M_PI_2);
            std::cout << "bumper warning， line_k= " << line_k << " global_goal_angle= " << global_goal_angle
                      << std::endl;
            WARNING_FLAG = 2;
            return true;
        }
        else
            WARNING_FLAG = 1;
    }
    else if (local_point_cloud.size() < 3)
    {
        WARNING_FLAG = 0;
        return false;
    }
    else WARNING_FLAG = 1;
    double min_dis = 999999, dis_tmp;
    int id = -1;
    for (int i = 0; i < local_point_cloud.size(); i++)
    {
        if (local_point_cloud[i].GetPoint()(0) / fabs(local_point_cloud[i].GetPoint()(1)) >= TAN30)
        {
            dis_tmp = pow(local_point_cloud[i].GetPoint()(0), 2) + pow(local_point_cloud[i].GetPoint()(1), 2);
            if (dis_tmp < min_dis)
            {
                id = i;
                min_dis = dis_tmp;
            }
        }
    }
    if (id > 0)
    {
        double local_angle = std::atan2(local_point_cloud[id].GetPoint()(1), local_point_cloud[id].GetPoint()(0));
        global_goal_angle = Funcs::NormalizeAngle(DataSet::robot_pose.angle() + local_angle + M_PI_2);
        std::cout << "bumper warning， global_goal_angle= " << global_goal_angle << " local_angle= " << local_angle
                  << std::endl;
        return true;
    }
    else
    {
        WARNING_FLAG = 0;
        return false;
    }
}

bool BumperPlanner::CollisionWarning(double distance_front, double distance_left)
{
    return Check(distance_front, distance_left);
}


} // namespace behavior_controller
